import os
import sys
import cv2
import math
import numpy as np
import serial
import time
import threading
import queue

data_received = ""  # 定义data_received为全局变量，初始为空字符串
# 创建一个队列用于存储待发送的数据
data_queue = queue.Queue()

# 定义要提取的颜色范围（以红色为例）
lower_red = np.array([0, 40, 100])  # HSV中红色的低阈值
upper_red = np.array([10, 255, 255])  # HSV中红色的高阈值
lower_red2 = np.array([160, 40, 100])  # HSV中红色的低阈值（对于H值在0-180之间的情况）
upper_red2 = np.array([180, 255, 255])  # HSV中红色的高阈值（对于H值在0-180之间的情况）
# 定义绿色范围
lower_green = np.array([40, 40, 40])  # 请根据需要进行调整
upper_green = np.array([80, 255, 255])  # 请根据需要进行调整

# 打开串口
ser = serial.Serial("/dev/ttyAMA0", 115200)
def serial_receiver():
    global data_received  # 使用global关键字声明data_received为全局变量
    while True:
        try:
            data_received = ser.readline().decode().strip()  # 从串口读取数据
            # 在这里可以处理接收到的数据，根据需要进行相应的操作
            # 例如，可以打印接收到的数据
            
            #print("Received data:", data_received)
        except Exception as e:
            print("Error while receiving data:", str(e))
            break
def serial_sender():
    while True:
        data = data_queue.get()  # 从队列中获取数据
        if data is None:  # 使用 None 作为停止信号
            break
        # 去除数据中的空格
        data_without_spaces = data.replace(" ", "")
        data_cleaned = data_without_spaces.replace("(", "").replace(")", "").replace(",", "")
        data_to_send = data_cleaned.encode('utf-8') + b'\n'
        ser.write(data_to_send)

def capture_and_process_video():
    global data_received  # 使用global关键字声明data_received为全局变量
    # 打开摄像头
    cap = cv2.VideoCapture(0)
    # 设置感兴趣区域的范围
    x_start, y_start, width, height = 129,122,342,318

    # 检查摄像头是否正确打开
    if not cap.isOpened():
        print("错误：无法打开摄像头。")
        return
    # 创建并启动串口接收线程
    receiver_thread = threading.Thread(target=serial_receiver)
    receiver_thread.start()
    # 创建并启动串口发送线程
    sender_thread = threading.Thread(target=serial_sender)
    sender_thread.start()
    #fps
    frame_count = 0
    start_time = time.time()
    fps = 0  # 初始化 fps 变量

    while True:
        # 捕获一帧图像
        ret, frame = cap.read()
        if not ret:
            print("错误：无法读取帧。")
            break

        # 选择感兴趣的区域
        roi = frame[y_start:y_start + height, x_start:x_start + width]
        # 对感兴趣区域进行处理
        '''识别最大四边形的'''
        detect_largest_rectangle(roi)
        '''识别红点'''
        detect_red_dot(roi, lower_red, upper_red)
        # 在原图上绘制感兴趣区域的边框
        cv2.rectangle(frame, (x_start, y_start), (x_start+width, y_start+height), (0, 255, 0), 2)
        # 计算帧率
        frame_count += 1
        if frame_count % 10 == 0:  # 每隔10帧计算一次帧率
            end_time = time.time()
            elapsed_time = end_time - start_time
            fps = frame_count / elapsed_time
            start_time = end_time
            frame_count = 0
        # 在图像上显示帧数信息
        cv2.putText(frame, f"FPS: {fps:.2f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
        # 显示图像
        cv2.imshow('Largest Black Rectangle Detection', frame)
        # 按 'q' 键退出
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    
    # 当你准备停止线程时，将 None 放入队列中
    data_queue.put(None)
    sender_thread.join()  # 等待线程完成
    receiver_thread.join()
    # 释放资源
    cap.release()
    cv2.destroyAllWindows()


def detect_red_dot(image, lower_red, upper_red):
    global data_received  # 使用global关键字声明data_received为全局变量
    '''识别红点'''
    # 将 BGR 转换为 HSV
    img_hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    # 使用颜色阈值找到指定颜色区域的掩码
    mask1 = cv2.inRange(img_hsv, lower_red, upper_red)
    mask2 = cv2.inRange(img_hsv, lower_red2, upper_red2)
    mask = cv2.bitwise_or(mask1, mask2)
    # 对原始图像和掩膜进行按位与操作，提取指定颜色的图像
    res = cv2.bitwise_and(image, image, mask=mask)
    # 将提取后的图像转换为灰度图像
    gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
    # 进行二值化处理
    _, thresholded_img = cv2.threshold(gray, 1, 255, cv2.THRESH_BINARY)
    # 查找图像中的轮廓
    contours, _ = cv2.findContours(thresholded_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours: # 没有找到轮廓，退出函数
        return    
    # 找到最大的轮廓
    max_contour = max(contours, key=cv2.contourArea)
    # 绘制中心点并显示坐标值
    M = cv2.moments(max_contour)
    if M["m00"] != 0:
        cX = int(M["m10"] / M["m00"])
        cY = int(M["m01"] / M["m00"])
        cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1)
        cv2.putText(image, f'({cX}, {cY})', (cX + 10, cY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
        
        # 将红点坐标转换为字符串并放入data_queue队列
        if (data_received=="@2b"):
            data_received=None
            red_dot_coords = f"({str(cX).zfill(3)},{str(cY).zfill(3)}"
            data_with_header = "#" + red_dot_coords
            data_queue.put(data_with_header)
            print(red_dot_coords)
        
def detect_green_dot(image, lower_green, upper_green):
    global data_received  # 使用global关键字声明data_received为全局变量
    # 将 BGR 转换为 HSV
    img_hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    # 使用颜色阈值找到指定颜色区域的掩码
    mask1 = cv2.inRange(img_hsv, lower_green, upper_green)
    # 对原始图像和掩膜进行按位与操作，提取指定颜色的图像
    res = cv2.bitwise_and(image, image, mask=mask1)
    # 将提取后的图像转换为灰度图像
    gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
    # 进行二值化处理
    _, thresholded_img = cv2.threshold(gray, 1, 255, cv2.THRESH_BINARY)
    # 查找图像中的轮廓
    contours, _ = cv2.findContours(thresholded_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours: # 没有找到轮廓，退出函数
        return       
    # 找到最大的轮廓
    max_contour = max(contours, key=cv2.contourArea)
    # 绘制中心点并显示坐标值
    M = cv2.moments(max_contour)
    if M["m00"] != 0:
        cX = int(M["m10"] / M["m00"])
        cY = int(M["m01"] / M["m00"])
        cv2.circle(image, (cX, cY), 5, (0, 255, 0), -1)
        cv2.putText(image, f'({cX}, {cY})', (cX + 10, cY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
        
        # 将红点坐标转换为字符串并放入data_queue队列
        green_dot_coords = f"({str(cX).zfill(3)},{str(cY).zfill(3)}"
        data_with_header = "#" + green_dot_coords
        data_queue.put(data_with_header)
        print(green_dot_coords)


def detect_largest_rectangle(image):
    global data_received  # 使用global关键字声明data_received为全局变量
    '''识别最大四边形的'''
    origin_point = (0, 0)  # 在图像上找到原点坐标 (0, 0)
    cv2.circle(image, origin_point, 5, (0, 0, 255), -1)  # 原点位置绘制红色圆点
    minarea_true = 500  # 最小值
    maxarea_true = 18000  # 最大值
    # 转换为灰度图像
    gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    # 高斯滤波平滑处理
    blurred_image = cv2.GaussianBlur(gray_image, (5, 5), 0)
    edges = cv2.Canny(blurred_image, 50, 150)
    # 找到轮廓
    contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    # 保留最大的黑色矩形
    largest_rectangle = None
    max_area = 0

    for cnt in contours:
        # 计算轮廓的面积
        area = cv2.contourArea(cnt)
        # 如果轮廓面积较小，可能不是我们要找的矩形，忽略
        if area < minarea_true or max_area > maxarea_true:
            continue

        # 判断是否为近似矩形
        peri = cv2.arcLength(cnt, True)
        approx = cv2.approxPolyDP(cnt, 0.02 * peri, True)

        if len(approx) == 4:
            if area > max_area:
                max_area = area
                largest_rectangle = approx
    if largest_rectangle is None: # 没有找到四边形，退出函数
        return

    # 如果检测到一个矩形，就绘制其角点并将角点坐标放入队列
    if (data_received=="@1b"):
        data_received=None
        if largest_rectangle is not None:
        # 提取角点坐标
            corners_str = ""
            for i,point in enumerate(largest_rectangle):
                x, y = point[0]
                corners_str += f"({str(x).zfill(3)},{str(y).zfill(3)})"
            # 去除最后一个逗号和空格
            corners_str = corners_str[:-1]
            data_with_header = "@" + corners_str
            print(data_with_header)
            data_queue.put(data_with_header)
    # 绘制角点
    for i, point in enumerate(largest_rectangle):
        x, y = point[0]
        cv2.circle(image, (x, y), 5, (128, 0, 0), -1)
        cv2.putText(image, f"{i + 1}:({str(x).zfill(3)},{str(y).zfill(3)})", (x + 10, y - 10),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (128, 100, 128), 1)
        # print(f"角点 {i+1}: ({x}, {y})")

    # Display the edge image and the original image with the rectangle marked
    cv2.imshow('Edges', edges)
    cv2.imshow('Largest Black Rectangle Detection', image)


if __name__ == "__main__":
    capture_and_process_video()
